//#include "stdafx.h"
#include "Utilities.h"

using namespace cv;
using namespace std;

char* file_bFOV( "D://Projects//Data//1.avi" );
char* file_sFOV_1( "D://Projects//Data//snapshots//1//BFOV_1.png" );
char* file_sFOV_2( "D://Projects//Data//snapshots//1//BFOV_3.png" );
char* window_bFOV( "big_FOV" );
char* window_sFOV_1( "small_FOV-1" );
char* window_sFOV_2( "small_FOV-2" );
char* frameTrackbar( "Position" );

char* file_ref1( "D:/Projects/Data/snapshots/1/ref1.png" );
char* file_noToolRef( "D:/Projects/Data/snapshots/1/noToolRef.png" );
char* file_toolRef( "D:/Projects/Data/snapshots/1/toolRef.png" );
char* file_ref1Mask( "D:/Projects/Data/snapshots/1/ref1Mask.png" );
char* file_noToolRefMask( "D:/Projects/Data/snapshots/1/noToolRefMask.png" );
char* file_toolRefMask( "D:/Projects/Data/snapshots/1/toolRefMask.png" );
char* file_ref1ToolMask( "D:/Projects/Data/snapshots/1/ref1ToolMask.png" );
char* file_ref1ShadowMask( "D:/Projects/Data/snapshots/1/ref1ShadowMask.png" );

int sliderPosition = 0;
VideoCapture capture = NULL;
void onFrameTrackbarSlide( int pos, void *userdata )
{
	//capture.set( CV_CAP_PROP_POS_FRAMES,pos );
	sliderPosition = pos;
	//cvSetCaptureProperty( capture,CV_CAP_PROP_POS_FRAMES,sliderPosition );
}

void somethingMask( Mat &inputImage, Mat &mask )
{
	Mat inputImageHSV, _r,_g,_b, _h,_s,_v;
	vector< Mat > channels;
	split( inputImage,channels );
	_b = channels[0];
	_g = channels[1];
	_r = channels[2];
	channels.clear();

	cvtColor( inputImage,inputImageHSV,CV_BGR2HSV_FULL );
	split( inputImageHSV,channels );
	_h = channels[0];
	_s = channels[1];
	_v = channels[2];
	channels.clear();

	getMask( _r,_h,mask );
}

int main(int argc, char* argv[])
{
	/*InitializeVector();
	Mat bRGB = imread( file_bFOV,CV_LOAD_IMAGE_COLOR );
	Mat sRGB_1 = imread( file_sFOV_1,CV_LOAD_IMAGE_COLOR );
	Mat sRGB_2 = imread( file_sFOV_2,CV_LOAD_IMAGE_COLOR );


	if( !( bRGB.data || sRGB_1.data || sRGB_2.data ) )
	{
		cout << "Could not open or find images, please check.." << endl;
		return -1;
	}

	Mat bHSV, sHSV_1, sHSV_2, bNewRGB, sNewRGB_1, sNewRGB_2;
	
	

	cvtColor( bRGB,bHSV,CV_BGR2HSV_FULL );
	cvtColor( sRGB_1,sHSV_1,CV_BGR2HSV_FULL );
	cvtColor( sRGB_2,sHSV_2,CV_BGR2HSV_FULL );

	namedWindow( window_bFOV,CV_WINDOW_NORMAL );
	namedWindow( window_sFOV_1,CV_WINDOW_NORMAL );
	namedWindow( window_sFOV_2,CV_WINDOW_NORMAL );

	/// Split image channels
	vector< Mat > chann_1,chann_2,chann_3,chann_4,chann_5,chann_6;
	split( bRGB,chann_1 );
	Mat bB = chann_1[0], bG = chann_1[1], bR = chann_1[2];
	split( bHSV,chann_2 );
	Mat bH = chann_2[0], bS = chann_2[1], bV = chann_2[2];		
	split( sRGB_1,chann_3 );
	Mat sB_1 = chann_3[0], sG_1 = chann_3[1], sR_1 = chann_3[2];
	split( sRGB_2,chann_4 );
	Mat sB_2 = chann_4[0], sG_2 = chann_4[1], sR_2 = chann_4[2];
	split( sHSV_1,chann_5 );
	Mat sH_1 = chann_5[0], sS_1 = chann_5[1], sV_1 = chann_5[2];
	split( sHSV_2,chann_6 );
	Mat sH_2 = chann_6[0], sS_2 = chann_6[1], sV_2 = chann_6[2];

	/// Create Mask
	Mat bMask,sMask_1,sMask_2;
	getMask( bR,bH,bMask );
	getMask( sR_1,sH_1,sMask_1 );
	getMask( sR_2,sH_2,sMask_2 );
	//gpu::GpuMat test;
	//test.upload( bMask );

	namedWindow( "test",CV_WINDOW_NORMAL );
	imshow( "test",bRGB );

	/// Keypoint Extraction		==========|
	vector< KeyPoint > kp_bG,kp_bV,kp_sG_1,kp_sG_2,kp_sV_1,kp_sV_2, kp_branch;
	// FAST
	/*kp_bG = keypointFAST( bB,bMask );
	kp_bV = keypointFAST( bV,bMask );
	kp_sG_1 = keypointFAST( sB_1,sMask_1 );
	kp_sV_1 = keypointFAST( sV_1,sMask_1 );
	kp_sG_2 = keypointFAST( sB_2,sMask_2 );
	kp_sV_2 = keypointFAST( sV_2,sMask_2 );*/

	// ORB
	/*kp_bG = keypointORB( bG,bMask );
	kp_bV = keypointORB( bV,bMask );
	kp_sG_1 = keypointORB( sG_1,sMask_1 );
	kp_sV_1 = keypointORB( sV_1,sMask_1 );
	kp_sG_2 = keypointORB( sG_2,sMask_2 );
	kp_sV_2 = keypointORB( sV_2,sMask_2 );*/

	// TOMASI
	/*kp_bG = keypointTomasi( bG,bMask );
	kp_bV = keypointTomasi( bV,bMask );
	kp_sG_1 = keypointTomasi( sG_1,sMask_1 );
	kp_sV_1 = keypointTomasi( sV_1,sMask_1 );
	kp_sG_2 = keypointTomasi( sG_2,sMask_2 );
	kp_sV_2 = keypointTomasi( sV_2,sMask_2 );*/

	// Branching Algo
	//kp_branch = getCircularROI( bV,bMask );

	// Drawing KP's
	//Mat out_bG,out_bV,out_sG_1,out_sG_2,out_sV_1,out_sV_2, out_bGRAY;
	/*drawKeypoints( bRGB,kp_bG,out_bG,Scalar::all(255) );
	drawKeypoints( bRGB,kp_bV,out_bV,Scalar::all(255) );
	drawKeypoints( sRGB_1,kp_sG_1,out_sG_1,Scalar::all(255) );
	drawKeypoints( sRGB_1,kp_sV_1,out_sV_1,Scalar::all(255) );
	drawKeypoints( sRGB_2,kp_sG_2,out_sG_2,Scalar::all(255) );
	drawKeypoints( sRGB_2,kp_sV_2,out_sV_2,Scalar::all(255) );
	drawKeypoints( bV,kp_branch,out_bGRAY,Scalar::all(255) );
	imshow( "test",out_bGRAY );
	imwrite( "test_Branch.png",out_bGRAY );
	//imshow( "KP_bG",out_bG );

	/// Keypoint Description	==========|
	Mat desc_bG,desc_bV,desc_sG_1,desc_sG_2,desc_sV_1,desc_sV_2;
	// BRISK
	/*desc_bG = descriptorBRISK( bB,kp_bG,bMask );
	desc_bV = descriptorBRISK( bV,kp_bV,bMask );
	desc_sG_1 = descriptorBRISK( sB_1,kp_sG_1,sMask_1 );
	desc_sG_2 = descriptorBRISK( sB_2,kp_sG_2,sMask_2 );
	desc_sV_1 = descriptorBRISK( sV_1,kp_sV_1,sMask_1 );
	desc_sV_2 = descriptorBRISK( sV_2,kp_sV_2,sMask_2 );*/

	// FREAK	
	/*desc_bG = descriptorFREAK( bG,kp_bG,bMask );
	desc_bV = descriptorFREAK( bV,kp_bV,bMask );
	desc_sG_1 = descriptorFREAK( sG_1,kp_sG_1,sMask_1 );
	desc_sG_2 = descriptorFREAK( sG_2,kp_sG_2,sMask_2 );
	desc_sV_1 = descriptorFREAK( sV_1,kp_sV_1,sMask_1 );
	desc_sV_2 = descriptorFREAK( sV_2,kp_sV_2,sMask_2 );*/

	// ORB 	
	/*desc_bG = descriptorORB( bG,kp_bG,bMask );
	desc_bV = descriptorORB( bV,kp_bV,bMask );
	desc_sG_1 = descriptorORB( sG_1,kp_sG_1,sMask_1 );
	desc_sG_2 = descriptorORB( sG_2,kp_sG_2,sMask_2 );
	desc_sV_1 = descriptorORB( sV_1,kp_sV_1,sMask_1 );
	desc_sV_2 = descriptorORB( sV_2,kp_sV_2,sMask_2 );*/

	// BRISK-KP+DESC
	/*keypointBRISK( bG,bMask,desc_bG,kp_bG );
	keypointBRISK( bV,bMask,desc_bV,kp_bV );
	keypointBRISK( sG_1,bMask,desc_sG_1,kp_sG_1 );
	keypointBRISK( sV_1,bMask,desc_sV_1,kp_sV_1 );
	keypointBRISK( sG_2,bMask,desc_sG_2,kp_sG_2 );
	keypointBRISK( sV_2,bMask,desc_sV_2,kp_sV_2 );

	Mat out_bG,out_bV,out_sG_1,out_sG_2,out_sV_1,out_sV_2;
	drawKeypoints( bRGB,kp_bG,out_bG,Scalar::all(255) );
	drawKeypoints( bRGB,kp_bV,out_bV,Scalar::all(255) );
	drawKeypoints( sRGB_1,kp_sG_1,out_sG_1,Scalar::all(255) );
	drawKeypoints( sRGB_1,kp_sV_1,out_sV_1,Scalar::all(255) );
	drawKeypoints( sRGB_2,kp_sG_2,out_sG_2,Scalar::all(255) );
	drawKeypoints( sRGB_2,kp_sV_2,out_sV_2,Scalar::all(255) );*/

	/// Keypoint Matching		==========|
	// BruteForce
	/*
	NORM_L1, NORM_L2, NORM_HAMMING, NORM_HAMMING2. L1 and L2 norms are preferable choices for SIFT and SURF descriptors, 
	NORM_HAMMING should be used with ORB, BRISK and BRIEF, NORM_HAMMING2 should be used with ORB when WTA_K==3 or 4.
	*/
	//BFMatcher matcher( NORM_HAMMING );

	// Flann
	//FlannBasedMatcher matcher;
	
	/*vector< DMatch > match_G_1,match_G_2,match_V_1,match_V_2;
	matcher.match( desc_sG_1,desc_bG,match_G_1 );
	matcher.match( desc_sG_2,desc_bG,match_G_2 );
	matcher.match( desc_sV_1,desc_bV,match_V_1 );
	matcher.match( desc_sV_2,desc_bV,match_V_2 );*/


	/// Draw Matches	==========|
	/*Mat img_match_G_1,img_match_G_2,img_match_V_1,img_match_V_2;
	drawMatches( sRGB_1,kp_sG_1, bRGB,kp_bG, match_G_1, img_match_G_1 );
	drawMatches( sRGB_2,kp_sG_2, bRGB,kp_bG, match_G_2, img_match_G_2 );
	drawMatches( sRGB_1,kp_sV_1, bRGB,kp_bV, match_V_1, img_match_V_1 );
	drawMatches( sRGB_2,kp_sV_2, bRGB,kp_bV, match_V_2, img_match_V_2 );

	namedWindow( "Green_1",CV_WINDOW_NORMAL );
	namedWindow( "Green_2",CV_WINDOW_NORMAL );
	namedWindow( "Value_1",CV_WINDOW_NORMAL );
	namedWindow( "Value_2",CV_WINDOW_NORMAL );

	imshow( "Green_1",img_match_G_1 );
	imshow( "Green_2",img_match_G_2 );
	imshow( "Value_1",img_match_V_1 );
	imshow( "Value_2",img_match_V_2 );

	imwrite( "bigFOV.png",bRGB );
	imwrite( "smallFOV_1.png",sRGB_1 );
	imwrite( "smallFOV_2.png",sRGB_2 );
	imwrite( "gFAST_Branch.png",out_bG );
	imwrite( "vFAST_Branch.png",out_bV );
	imwrite( "gFAST_1.png",out_sG_1 );
	imwrite( "vFAST_1.png",out_sV_1);
	imwrite( "gFAST_2.png",out_sG_2 );
	imwrite( "vFAST_2.png",out_sV_2 );
	imwrite( "gFAST-BRISK-BF_1.png",img_match_G_1 );
	imwrite( "gFAST-BRISK-BF_2.png",img_match_G_2 );
	imwrite( "vFAST-BRISK-BF_1.png",img_match_V_1 );
	imwrite( "vFAST-BRISK-BF_2.png",img_match_V_2 );*/

	/// Section to draw only "GOOD" matches
	////-- Quick calculation of max and min distances between keypoints
	//for( int i = 0; i < descriptors_1.rows; i++ )
	//{ double dist = matches[i].distance;
	//if( dist < min_dist ) min_dist = dist;
	//if( dist > max_dist ) max_dist = dist;
	//}
	//
	//printf("-- Max dist : %f \n", max_dist );
	//printf("-- Min dist : %f \n", min_dist );
	//
	////-- Draw only "good" matches (i.e. whose distance is less than 2*min_dist )
	////-- PS.- radiusMatch can also be used here.
	//std::vector< DMatch > good_matches;
	//
	//for( int i = 0; i < descriptors_1.rows; i++ )
	//{ if( matches[i].distance < 2*min_dist )
	//{ good_matches.push_back( matches[i]); }
	//}
	//
	////-- Draw only "good" matches
	//Mat img_matches;
	//drawMatches( img_1, keypoints_1, img_2, keypoints_2,
	//		   good_matches, img_matches, Scalar::all(-1), Scalar::all(-1),
	//		   vector<char>(), DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );
	//
	////-- Show detected matches
	//imshow( "Good Matches", img_matches );

	//Point loc(300,600 );
	//
	//namedWindow( "full image",CV_WINDOW_NORMAL );
	//imshow( "full image",bG );
	//CircularROI roi = getCircularROI(bRGB, loc/*, 50*/ );

	
	/*Mat ref1 = imread( file_ref1,CV_LOAD_IMAGE_COLOR );
	Mat noToolRef = imread( file_noToolRef,CV_LOAD_IMAGE_COLOR );
	Mat toolRef = imread( file_toolRef,CV_LOAD_IMAGE_COLOR );

	cv::Size s( ref1.size().width, ref1.size().height );

	Mat ref1Mask,noToolRefMask,toolRefMask,ref1ToolMask,ref1ShadowMask;
	
	somethingMask( ref1,ref1Mask );
	imwrite( file_ref1Mask,ref1Mask );
	somethingMask( noToolRef,noToolRefMask );
	imwrite( file_noToolRefMask,noToolRefMask );
	somethingMask( toolRef,toolRefMask );
	imwrite( file_toolRefMask,toolRefMask );

	ref1ShadowMask = Mat::zeros( s,ref1.depth() );
	imwrite( file_ref1ShadowMask,ref1ShadowMask );*/

	
	VideoCapture capture( file_bFOV );/*
	namedWindow( window_bFOV, CV_WINDOW_NORMAL );
	createTrackbar( frameTrackbar,window_bFOV,&sliderPosition,capture.get(CV_CAP_PROP_FRAME_COUNT),
		onFrameTrackbarSlide );*/

	for( ;; )
	{
		//setTrackbarPos( frameTrackbar,window_bFOV, ++sliderPosition);

		Mat frameRGB,frameHSV, mask, out_frame, frameRGB_2,frameHSV_2, mask_2, out_frame_2;
		capture >> frameRGB;
		/*capture.set(CV_CAP_PROP_FRAME_COUNT,sliderPosition+1 );
		capture >> frameRGB_2;*/

		vector< Mat > channels_1_rgb,channels_1_hsv,channels_2_rgb,channels_2_hsv,channel_test;
		//Mat f_R, f_B, f_G, f_H, f_S, f_V;
		split( frameRGB,channels_1_rgb );
		//split( frameRGB_2,channels_2_rgb );
		/*f_B = channels[0];
		f_G = channels[1];
		f_R = channels[2];
		channels.clear();*/
		//showImageChannels( "RGB",frameRGB,f_B,f_G,f_R );

		/*cvtColor( frameRGB,frameHSV,CV_BGR2HSV_FULL );
		split( frameHSV,channels_1_hsv );*/
		//cvtColor( frameRGB_2,frameHSV_2,CV_BGR2HSV_FULL );
		//split( frameHSV_2,channels_2_hsv );
		/*f_H = channels[0];
		f_S = channels[1];
		f_V = channels[2];
		channels.clear();*/
		//showImageChannels( "HSV",frameRGB,f_H,f_S,f_V );		
		
		getMask( channels_1_rgb[2],channels_1_rgb[1],cv::Mat());
		//getMask( channels_2_rgb[2],channels_2_hsv[0], mask_2);
		
		Mat test/* = 0.5*channels_1_rgb[1] + 0.25*channels_1_rgb[2]*/;
		normalize( channels_1_rgb[1],test, 0,255,NORM_MINMAX );
		//convolveDFT( channels_1_rgb[1],channels_1_rgb[2], test );
		//filter2D( channels_1_rgb[1],test,-1,channels_1_rgb[2] );

		//normalize( channels_1_rgb[1],test,0,255 );

		vector< KeyPoint > kp = keypointFAST( test,mask );
		/*vector< KeyPoint > kp_r = keypointFAST( channels_1_rgb[2],mask );
		kp.insert( kp.end(), kp_r.begin(),kp_r.end() );*/
		//vector< KeyPoint > kp_2 = keypointFAST( channels_2_rgb[0],mask_2 );
		//vector< KeyPoint > kp = keypointTomasi( channels_1_rgb[1],mask );

		drawKeypoints( frameRGB,kp,out_frame, Scalar::all( 255 ) );
		/*drawKeypoints( out_frame,kp_r,out_frame, Scalar( 0,0,255 ) );
		drawKeypoints( out_frame,kp_b,out_frame, Scalar( 255,0,0 ) );*/

		Mat desc_1, desc_2, img_match;
		char filename[80];
		int count = 0;

		if( kp.size() /*!= kp_2.size()*/ <= 0 )
		{
			namedWindow( window_bFOV, CV_WINDOW_NORMAL );
			createTrackbar( frameTrackbar,window_bFOV,&sliderPosition,capture.get(CV_CAP_PROP_FRAME_COUNT),
				onFrameTrackbarSlide );
			setTrackbarPos( frameTrackbar,window_bFOV, ++sliderPosition);

			//desc_1 = descriptorBRISK( channels_1_rgb[0],kp,mask );
			//desc_2 = descriptorBRISK( channels_2_rgb[0],kp,mask_2 );
			
			//BFMatcher matcher( NORM_HAMMING );
			
			/*vector< DMatch > matches;
			matcher.match( desc_1,desc_2,matches );
			drawMatches( frameRGB,kp, frameRGB_2,kp_2, matches, img_match,
				Scalar(255,0,0),Scalar(0,0,255) );*/

			//imshow( window_bFOV,img_match );
			imshow( window_bFOV,out_frame );


			cvWaitKey();

		}
		else
		{
			namedWindow( window_bFOV, CV_WINDOW_NORMAL );
			createTrackbar( frameTrackbar,window_bFOV,&sliderPosition,capture.get(CV_CAP_PROP_FRAME_COUNT),
				onFrameTrackbarSlide );
			setTrackbarPos( frameTrackbar,window_bFOV, ++sliderPosition);
			imshow( window_bFOV,out_frame );
		}
		
		if( cvWaitKey(10) > 26 )
		{
			if( cvWaitKey() == 32 )
			{
				/*while(1)
				{*/
				/*count = sliderPosition;
				sprintf( filename, "frame_%d.png",count );
				imwrite( filename,out_frame );
				out_frame.release();
				vector< KeyPoint > kp = keypointFAST( channels_1_rgb[0],mask );
				drawKeypoints( frameRGB,kp,out_frame, Scalar::all( 255 ) );
				namedWindow( "test",CV_WINDOW_NORMAL );
				imshow( "test",out_frame );*/
				cvWaitKey();
				//}
				goto NEXT;
			}
			else
			break;
		}

		NEXT:
		capture.set( CV_CAP_PROP_POS_FRAMES,sliderPosition+5 );
		//count = sliderPosition;
	}

	destroyAllWindows();

	return 0;
}

